#include "targer_tracker/target_tracker.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "algorithm"
#include  "numeric"
#include "math.h"
#include <algorithm>

target_tracker::target_tracker():Node("target_tracker_node")
{
    // ---------------------------------------declear parameters ------------------------------------
    this->declare_parameter<std::string>("subscribers.uav_state_sub.topic", "/mavros/state");
    this->declare_parameter<int>("subscribers.uav_state_sub.queue_size", 10);
    
    this->declare_parameter<std::string>("subscribers.trackerid_sub.topic", "/detection_image/result_box");
    this->declare_parameter<int>("subscribers.trackerid_sub.queue_size", 10);
    
    this->declare_parameter<std::string>("subscribers.location_sub.topic", "/mavros/local_position/pose");

    this->declare_parameter<std::string>("subscribers.call_server.topic", "/drone_target_tracker_server_topic_signal");
    this->declare_parameter<std::string>("subscribers.call_server.topic_mutile", "/drone_target_tracker_server_topic_mutile");

    this->declare_parameter<std::string>("client.arming_client.topic", "/mavros/cmd/arming");
    this->declare_parameter<std::string>("client.set_mode_client.topic", "/mavros/set_mode");
    this->declare_parameter<std::string>("client.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");

    this->declare_parameter<std::string>("services.calldetectserver.names", "drone_target_tracker_server");

    this->declare_parameter<double>("params.pid_x.kp", 0.5);
    this->declare_parameter<double>("params.pid_x.ki", 0.01);
    this->declare_parameter<double>("params.pid_x.kd", 0.05);

    this->declare_parameter<double>("params.pid_y.kp", 0.5);
    this->declare_parameter<double>("params.pid_y.ki", 0.01);
    this->declare_parameter<double>("params.pid_y.kd", 0.05);

    this->declare_parameter<int>("params.tracker_id.id", 1);
    this->declare_parameter<std::string>("params.tracker_id.names", "tank");
    this->declare_parameter<double>("params.tracker_id.delta_t", 0.1);

    this->declare_parameter<double>("params.image_width.value", 1920.0);
    this->declare_parameter<double>("params.image_height.value", 1080.0);
    this->declare_parameter<double>("params.target_location_width.value", 960.0);
    this->declare_parameter<double>("params.target_location_height.value", 540.0);
    
    this->declare_parameter<int>("params.vehicle_id", 1);
    this->declare_parameter<std::string>("params.namesspace", "drone_2");
    this->declare_parameter<std::string>("publishers.dingwei_publisher","target_pub");
    this->declare_parameter<double>("params.max_vel.value", 3.0);

    // 无人机是否进行追踪的状态话题名称
    this->declare_parameter<std::string>("publishers.tracker_status","tracker_status");
    // ---------------------------------------read parameters ---------------------------------------

    namespaces = this->get_parameter("params.namesspace").as_string();
    uav_state_sub_topic_ = this->get_parameter("subscribers.uav_state_sub.topic").as_string();
    uav_state_sub_topic_ = namespaces + uav_state_sub_topic_;
    uav_state_sub_queue_size_ = this->get_parameter("subscribers.uav_state_sub.queue_size").as_int();

    trackerid_sub_topic_ = this->get_parameter("subscribers.trackerid_sub.topic").as_string();
    trackerid_sub_topic_ = namespaces + trackerid_sub_topic_;
    trackerid_sub_queue_size_ = this->get_parameter("subscribers.trackerid_sub.queue_size").as_int();


    locaiton_sub_ = this->get_parameter("subscribers.location_sub.topic").as_string();
    locaiton_sub_ = namespaces + locaiton_sub_;
    call_server_topic_name = this->get_parameter("subscribers.call_server.topic").as_string();
    call_server_topic_name = namespaces + call_server_topic_name;

    multi_call_server_topic_name = this->get_parameter("subscribers.call_server.topic_mutile").as_string();
    multi_call_server_topic_name = namespaces + multi_call_server_topic_name;


    arming_client_topic_ = this->get_parameter("client.arming_client.topic").as_string();
    arming_client_topic_ = namespaces + arming_client_topic_;
    set_mode_client_topic_ = this->get_parameter("client.set_mode_client.topic").as_string();
    set_mode_client_topic_ = namespaces + set_mode_client_topic_;
    velocity_pub_topic_ = this->get_parameter("client.velocity_pub.topic").as_string();
    velocity_pub_topic_ = namespaces + velocity_pub_topic_;
    detect_server_ = this->get_parameter("services.calldetectserver.names").as_string();
    detect_server_ = namespaces + detect_server_;

    pid_x__Kp = this->get_parameter("params.pid_x.kp").as_double();
    pid_x__Ki = this->get_parameter("params.pid_x.ki").as_double();
    pid_x__Kd = this->get_parameter("params.pid_x.kd").as_double();

    pid_y__Kp = this->get_parameter("params.pid_y.kp").as_double();
    pid_y__Ki = this->get_parameter("params.pid_y.ki").as_double();
    pid_y__Kd = this->get_parameter("params.pid_y.kd").as_double();

    tracker_id = this->get_parameter("params.tracker_id.id").as_int();
    tracker_name_ = this->get_parameter("params.tracker_id.names").as_string();
    delta_t = this->get_parameter("params.tracker_id.delta_t").as_double();

    max_vel = this->get_parameter("params.max_vel.value").as_double();

    w = this->get_parameter("params.image_width.value").as_double();
    h = this->get_parameter("params.image_height.value").as_double();
    target_location_width = this->get_parameter("params.target_location_width.value").as_double();
    target_location_height = this->get_parameter("params.target_location_height.value").as_double();
    dingwei_topic_name = this->get_parameter("publishers.dingwei_publisher").as_string();
    dingwei_topic_name = namespaces + dingwei_topic_name;
    vehcile_id_ = this->get_parameter("params.vehicle_id").as_int();

    // 无人机的追踪状态发布话题名称
    tracker_status_topic_ = this->get_parameter("publishers.tracker_status").as_string();
    tracker_status_topic_ = namespaces + tracker_status_topic_;
    
    first_find= false;
    info_topic();
    if (call_server_topic_name.empty()||uav_state_sub_topic_.empty() || trackerid_sub_topic_.empty() || arming_client_topic_.empty() || set_mode_client_topic_.empty() || velocity_pub_topic_.empty()||detect_server_.empty()||tracker_name_.empty() || locaiton_sub_.empty())
    {
        RCLCPP_ERROR(this->get_logger(), "parameter is empty");
        rclcpp::shutdown();
    }

    pid_x_.resetvalue(pid_x__Kp, pid_x__Ki, pid_x__Kd);
    pid_y_.resetvalue(pid_y__Kp, pid_y__Ki, pid_y__Kd);
    pid_x_.set_decayfactor(50);
    pid_y_.set_decayfactor(50);
    filter_x_.setWindowSize(5);
    filter_y_.setWindowSize(5);
    tracker_times = 0;


    b_arrived = false;

    std::string swarns = "/uav_swarms/leader/cmd_vel_flu";

    rclcpp::QoS qos = rclcpp::QoS(rclcpp::SensorDataQoS());
    state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
        uav_state_sub_topic_, uav_state_sub_queue_size_, std::bind(&target_tracker::state_callback, this, std::placeholders::_1));
    rclcpp::QoS detect_qos = rclcpp::QoS(rclcpp::SensorDataQoS());
    tracker_sub_ = this->create_subscription<yolo_detect_message::msg::DetectBoxs> (
        trackerid_sub_topic_,rclcpp::QoS(10).best_effort(),std::bind(&target_tracker::tracker_object, this, std::placeholders::_1));
    // 初始化服务和发布者
    // local_pos_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
    //     "mavros/setpoint_position/local", 10);
    arming_client_ = this->create_client<mavros_msgs::srv::CommandBool>(arming_client_topic_);
    set_mode_client_ = this->create_client<mavros_msgs::srv::SetMode>(set_mode_client_topic_);
    velocity_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(velocity_pub_topic_, 10);
    
    pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
       locaiton_sub_, qos, std::bind(&target_tracker::pose_sub, this, std::placeholders::_1));

    dingwei_pub_ = this->create_publisher<client_interfaces::msg::ConfigTarget>(dingwei_topic_name, 10);

    // 位置相关的控制接口
    std::string move_pub_str_ = "/uav_swarms" + namespaces + "/move";
    move_pub_ = this->create_publisher<yolo_detect_message::msg::MoveInterface>(move_pub_str_,10);

    timer_ = this->create_wall_timer(std::chrono::milliseconds(50),std::bind(&target_tracker::timer_loop,this));
    
    callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    rclcpp::QoS qos_profile(rclcpp::KeepLast(10)); 
    qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); // 使用 BEST_EFFORT 模式 
    qos_profile.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE);
    // 无人机追踪状态发布者构建
    tracker_status_pub_ = this->create_publisher<std_msgs::msg::Bool>(tracker_status_topic_,10);
    service_ = this->create_service<yolo_detect_message::srv::Service>(detect_server_, std::bind(&target_tracker::service_callback,this,std::placeholders::_1,std::placeholders::_2),rmw_qos_profile_services_default,callback_group_);
    service_status = false;
    service_status_multi = false;
    service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(call_server_topic_name,rclcpp::QoS(10).best_effort(),std::bind(&target_tracker::service_status_callback,this,std::placeholders::_1));
    multi_service_status_subscriber_ = this->create_subscription<yolo_detect_message::msg::CallServerStatus>(multi_call_server_topic_name,rclcpp::QoS(10).best_effort(),std::bind(&target_tracker::multi_service_status_callback,this,std::placeholders::_1));
    target_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(swarns,10);
}

target_tracker::~target_tracker()
{
    RCLCPP_INFO(this->get_logger(),"node finished!");
    
}

void target_tracker::tracker_object(const yolo_detect_message::msg::DetectBoxs::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(),"GET INFORMATION");
    float target_x = 0.0;
    float target_y = 0.0;
    int center_x = w / 2.0;
    int center_y = h / 2.0;
    double vel_y;
    double vel_x;
    b_find = false;

    if (msg->detect_boxes.empty())
    {
        reset_velocity();
        return;
    }
    std::cout<<"-------------------------start detect tank------------------"<<std::endl;
    yolo_detect_message::msg::DetectBoxs conttains_target;
    for (size_t i = 0; i < msg->detect_boxes.size(); i++)
    {
        if (msg->detect_boxes[i].id == 6)
        {
            conttains_target.detect_boxes.push_back(msg->detect_boxes[i]);
        }
    }
    if(!conttains_target.detect_boxes.empty()) {
        auto compare = std::bind(&target_tracker::compareBydistance, this, std::placeholders::_1, std::placeholders::_2);
        std::sort(conttains_target.detect_boxes.begin(), conttains_target.detect_boxes.end(), compare);
        yolo_detect_message::msg::DetectBox box = conttains_target.detect_boxes[0];
        wait_time = 0;
        first_find = true;
        target_x = box.xmin + box.width / 2.0;
        target_y = box.ymin + box.heigh / 2.0;  
        b_find = true;
        tracker_times = 0;

    }

    if (!b_find)
    {
        tracker_times++;
    }
    else
    {
        vel_y = pid_y_.calculate(target_location_width, target_x, delta_t, w);
        vel_x = pid_x_.calculate(target_location_height, target_y, delta_t, h);

        double max_value_pid = sqrt(vel_x * vel_x + vel_y * vel_y);
        if (max_value_pid > max_vel)
        {
            vel_x = vel_x / max_value_pid * max_vel;
            vel_y = vel_y / max_value_pid * max_vel;
        }

        vel_x = clamp(vel_x, max_vel);
        vel_y = clamp(vel_y, max_vel);
        set_velocity(vel_x, vel_y);

        RCLCPP_INFO(this->get_logger(), "PID velocities set (x: %f, y: %f)", vel_x, vel_y);
    }

    if (tracker_times >= 60)
    {
        reset_velocity();
    }
}

void target_tracker::state_callback(const mavros_msgs::msg::State::SharedPtr msg)
{
    // std::lock_guard<std::mutex> current_states_(current_state_mutex);
    RCLCPP_INFO(this->get_logger(),"received infomation");
    current_state_ = *msg;
}


void target_tracker::mutil_state_callback(const mavros_msgs::msg::State::SharedPtr msg)
{
    // std::lock_guard<std::mutex> current_states_(current_state_mutex);
    RCLCPP_INFO(this->get_logger(),"received infomation");
    current_state_ = *msg;
}
void target_tracker::arm_drone()
{
    if (!arming_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Arming service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
    request->value = true;  // 发送ARM命令

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = arming_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedFuture result) {
            if (result.get()->success)
            {
                RCLCPP_INFO(this->get_logger(), "Drone armed successfully");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to arm drone");
            }
        });
}

void target_tracker::set_offboard_mode()
{
    if (!set_mode_client_->wait_for_service(std::chrono::seconds(1)))
    {
        RCLCPP_ERROR(this->get_logger(), "Set mode service not available");
        return;
    }

    auto request = std::make_shared<mavros_msgs::srv::SetMode::Request>();
    request->custom_mode = "OFFBOARD";

    // 异步发送请求，并使用回调函数处理结果
    auto result_future = set_mode_client_->async_send_request(request,
        [this](rclcpp::Client<mavros_msgs::srv::SetMode>::SharedFuture result) {
            if (result.get()->mode_sent)
            {
                RCLCPP_INFO(this->get_logger(), "OFFBOARD mode enabled");
            }
            else
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to enable OFFBOARD mode");
            }
        });
}


void target_tracker::timer_loop()
{
    if (service_status == false && service_status_multi == false)
    {
        RCLCPP_ERROR(this->get_logger(), "service is not ready,service_status is %s",service_status ? "true":"false");
        return;
    }

    // 发布速度控制命令
    if (first_find== false)
    {
        reset_velocity();
    }
    if (wait_time> 25)
    {
        reset_velocity();
    }
    // 如果丢失的时间太长，则向地面站发布无人的状态丢失
    if (wait_time   > 210  )
    {
        std_msgs::msg::Bool tracker_statuss;
        tracker_statuss.data = false;
        tracker_status_pub_->publish(tracker_statuss);
    } else if (first_find == true && wait_time < 210){
        std_msgs::msg::Bool tracker_statuss;
        tracker_statuss.data = true;
        tracker_status_pub_->publish(tracker_statuss);
    }
    
    wait_time ++;
    RCLCPP_INFO(this->get_logger(),"wait times is %d",wait_time);
    double vel_x = filter_x_.filter(current_velocity.linear.x);
    double vel_y = filter_y_.filter(current_velocity.linear.y);
    set_velocity(vel_x,vel_y);
    if ( service_status == true)
    {
        yolo_detect_message::msg::MoveInterface msg;
        msg.is_body = true;
        msg.is_vel = true;
        // 使用XYZ坐标轴的控制
        msg.is_gps = false;
        msg.pose = geometry_msgs::msg::Pose();
        msg.vel = current_velocity;
        move_pub_->publish(msg);

        RCLCPP_INFO(this->get_logger(),"tracker publish velocity x= %f,y = %f",current_velocity.linear.x,current_velocity.linear.y);
    }
    if (service_status_multi == true)
    {
        target_vel_pub_->publish(current_velocity);
        RCLCPP_INFO(this->get_logger(),"multi tracker publish velocity");
        
    }
    RCLCPP_INFO(this->get_logger(),"control information has published");
}

void target_tracker::reset_velocity(){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(0.0);
    current_velocity.linear.set__y(0.0);
    current_velocity.linear.set__z(0.0);
}

void target_tracker::set_velocity(float x, float y){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(x);
    current_velocity.linear.set__y(y);
    current_velocity.linear.set__z(0.0);
}

float target_tracker::clamp(float value,float max_value){
    float result= {};
    if (value > max_value)
    {
        result = max_value;
    }else if (value < (-1.0 * max_value))
    {
        result = -1.0 * max_value;
    }else{
        result = value;
    }
    return result;
}


void target_tracker::pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg){
    RCLCPP_INFO(this->get_logger(),"receiev posisiton");
    UAV_position = *msg;
}

void target_tracker::set_velocity_z(float z){
    std::lock_guard<std::mutex> current_velocity_mutex_lock(current_velocity_mutex);
    current_velocity.angular.set__x(0.0);
    current_velocity.angular.set__y(0.0);
    current_velocity.angular.set__z(0.0);

    current_velocity.linear.set__x(0.0);
    current_velocity.linear.set__y(0.0);
    current_velocity.linear.set__z(z);
}

void target_tracker::service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res){
    bool reqs = req->status;
    if(reqs){
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback.");
    }
    else{
        RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] service callback false.");

    }
    {
        std::lock_guard<std::mutex> lock_image(service_status_mutex);
        service_status = reqs;
    }
    
    res->success = true;
    res->drone_statue = "target tracker service_status is " + std::to_string(service_status);
}



void target_tracker::service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  
  bool reqs = msg->status;
    if(reqs != service_status)
    {
        std::lock_guard<std::mutex> lock_image(service_status_mutex);
        service_status = reqs;
    }
    RCLCPP_INFO(this->get_logger(),"[target tracker node] service status is %d.",service_status);
}

void target_tracker::multi_service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg){
  
  bool reqs = msg->status;
    if(reqs != service_status_multi)
    {
        std::lock_guard<std::mutex> lock_image(multi_service_status_mutex);
        service_status_multi = reqs;
    }
    RCLCPP_INFO(this->get_logger(),"[target tracker node] mutil service status is %d.",service_status);
}

void target_tracker::info_topic(){
  RCLCPP_INFO(this->get_logger(),"namespaces: %s",namespaces.c_str());
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] info topic. start.");
  RCLCPP_INFO(this->get_logger(), "vehicle id: %d", vehcile_id_);
  RCLCPP_INFO(this->get_logger(),"uav_state_sub_topic_: %s",uav_state_sub_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"trackerid_sub_topic_: %s",trackerid_sub_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"arming_client_topic_: %s",arming_client_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"set_mode_client_topic_: %s",set_mode_client_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"velocity_pub_topic_: %s",velocity_pub_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"velocity_pub_topic_: %s",velocity_pub_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"detect_server_: %s",detect_server_.c_str());
  RCLCPP_INFO(this->get_logger(),"dingwei_topic_name: %s",dingwei_topic_name.c_str());

    RCLCPP_INFO(this->get_logger(),"pid_x__Kp: %f",pid_x__Kp);
    RCLCPP_INFO(this->get_logger(),"pid_x__Ki: %f",pid_x__Ki);
    RCLCPP_INFO(this->get_logger(),"pid_x__Kd: %f",pid_x__Kd);
    RCLCPP_INFO(this->get_logger(),"pid_y__Kp: %f",pid_y__Kp);
    RCLCPP_INFO(this->get_logger(),"pid_y__Ki: %f",pid_y__Ki);
    RCLCPP_INFO(this->get_logger(),"pid_y__Kd: %f",pid_y__Kd);
    RCLCPP_INFO(this->get_logger(),"tracker_status_topic_: %s",tracker_status_topic_.c_str());
  RCLCPP_INFO(this->get_logger(),"[YoloObjectDetector] info topic. edd.");
}